#include "rclcpp/rclcpp.hpp"
#include <QApplication>
#include <QWidget>
#include <QProcess>
#include "control_plane.h"
#include <QPushButton>
#include <QMetaObject>
#include "std_msgs/msg/float32_multi_array.hpp"
#include "std_msgs/msg/float32.hpp"
#include <sensor_msgs/msg/imu.hpp>
#include <QTimer>
#include <QThread>

#include <memory>
#include <string>
#include <cstdlib>

float d=0.085;

class ControlPlaneV2 : public rclcpp::Node
{
    private:
        Ui::control_plane ui_;
        QWidget window_;
        void onPowerOff()
        {
            RCLCPP_INFO(this->get_logger(), "power off. Nodes are closing.");
            // QProcess::startDetached("/userdata/nn_car4/node_close.sh");
            // 关闭ROS 2节点
            rclcpp::shutdown();
            QProcess::startDetached("sudo shutdown -h now");
        }
        void onRFNNStart()
        {
            QProcess::startDetached("/userdata/nn_car4/nn_car_on2.sh");
        }
        void onESP32Start()
        {
             QProcess::startDetached("/userdata/nn_car4/esp32_on.sh");
        }
        void onSwitchOff()
        {
            // 发布当前节点的状态
            RCLCPP_INFO(this->get_logger(), "Switching off. Nodes are closing.");
            QProcess::startDetached("/userdata/nn_car4/node_close.sh");
            // // 关闭ROS 2节点
            // rclcpp::shutdown();
            // // 退出应用程序
            // QApplication::quit();
        }
        rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr data_subscribe_;
        void data_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
        {
            RCLCPP_INFO(this->get_logger(), "已接收");
            float dg_val = msg->data[0];
            float sig_val = msg->data[1];
            float do_val = msg->data[2];
            float sio_val = msg->data[3];
            // 
            auto dg_str = QString::number(dg_val);
            auto sig_str = QString::number(sig_val);
            auto do_str = QString::number(do_val);
            auto sio_str = QString::number(sio_val);
            // 使用Lambda表达式直接更新UI
            ui_.dg_val->setText(dg_str);
            ui_.sig_val->setText(sig_str);
            ui_.do_val->setText(do_str);
            ui_.sio_val->setText(sio_str);
            // QMetaObject::invokeMethod(&ui_, "updateLabels",
            // Q_ARG(QString,QString::number(dg_val)),
            // Q_ARG(QString,QString::number(sig_val)),
            // Q_ARG(QString,QString::number(do_val)),
            // Q_ARG(QString,QString::number(sio_val)));
        }
        rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr rpy_subscribe_;
        void rpy_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
        {
            RCLCPP_INFO(this->get_logger(), "imu已接收");
            float ang_roll=msg->data[0];
            float ang_pitch=msg->data[1];
            float ang_yaw=msg->data[2];
            //
            auto ang_roll_str=QString::number(ang_roll);
            auto ang_pitch_str=QString::number(ang_pitch);
            auto ang_yaw_str=QString::number(ang_yaw);
            //
            ui_.ang_roll->setText(ang_roll_str);
            ui_.ang_pitch->setText(ang_pitch_str);
            ui_.ang_yaw->setText(ang_yaw_str);
        }
        rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr pos_subscribe_;
        void pos_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
        {
            RCLCPP_INFO(this->get_logger(), "位置已接收");
            float pos_x=msg->data[0];
            float pos_y=msg->data[1];
            float pos_ang=msg->data[2];
            //
            auto pos_x_str=QString::number(pos_x);
            auto pos_y_str=QString::number(pos_y);
            auto pos_ang_str=QString::number(pos_ang);
            //
            ui_.pos_x->setText(pos_x_str);
            ui_.pos_y->setText(pos_y_str);
            ui_.pos_ang->setText(pos_ang_str);
        }
        rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr lidar_subscribe_;
        void lidar_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
        {
            RCLCPP_INFO(this->get_logger(), "雷达收到");
            float do_det = msg->data[0];
            float sio_det = msg->data[1];
            //
            auto do_det_str = QString::number(do_det);
            auto sio_det_str = QString::number(sio_det);
            // 使用Lambda表达式直接更新UI
            ui_.do_det->setText(do_det_str);
            ui_.sio_det->setText(sio_det_str);
        }
        //声明imu主题
        rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
        void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
        {
            float w=msg->angular_velocity.z;
            //
            auto w_str=QString::number(w);
            //
            ui_.z_ang_val->setText(w_str);
        }
        // 声明一个订阅者L
        rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr command_subscribe_L;
        // 声明一个订阅者R
        rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr command_subscribe_R;
        // 收到话题数据的回调函数L
        void vL_callback(const std_msgs::msg::Float32::SharedPtr msg)
        {
            float rpmL=msg->data;
            float vLo=rpmL*2*3.14/60.0*(d/2.0);
            auto vLo_str=QString::number(vLo);
            ui_.vLo_val->setText(vLo_str);
        }
        // 收到话题数据的回调函数R
        void vR_callback(const std_msgs::msg::Float32::SharedPtr msg)
        {
            float rpmR=msg->data;
            float vRo=rpmR*2*3.14/60.0*(d/2.0);
            auto vRo_str=QString::number(vRo);
            ui_.vRo_val->setText(vRo_str);
        }
        // 声明一个订阅者L
        rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr command_vL;
        // 声明一个订阅者R
        rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr command_vR;
        void cal_vL_callback(const std_msgs::msg::Float32::SharedPtr msg)
        {
            float rpmLd=msg->data;
            float vL=rpmLd*2*3.14/60.0*(d/2.0);
            auto vL_str=QString::number(vL);
            ui_.vL_val->setText(vL_str);
        }
        void cal_vR_callback(const std_msgs::msg::Float32::SharedPtr msg)
        {
            float rpmRd=msg->data;
            float vR=rpmRd*2*3.14/60.0*(d/2.0);
            auto vR_str=QString::number(vR);
            ui_.vR_val->setText(vR_str);
        }
    public:
        ControlPlaneV2(std::string name):Node(name)
        {
            RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
            ui_.setupUi(&window_);
            //imu
            rpy_subscribe_=this->create_subscription<std_msgs::msg::Float32MultiArray>(
                "in_rpy", 10, 
                std::bind(&ControlPlaneV2::rpy_callback, this, std::placeholders::_1));
            //机器人位置
            pos_subscribe_=this->create_subscription<std_msgs::msg::Float32MultiArray>(
                "pos_obs_des", 10, 
                std::bind(&ControlPlaneV2::pos_callback, this, std::placeholders::_1));
            //创建rfnn输入数据订阅者
            data_subscribe_=this->create_subscription<std_msgs::msg::Float32MultiArray>(
                "in_data", 10, 
                std::bind(&ControlPlaneV2::data_callback, this, std::placeholders::_1));
            //创建雷达数据订阅者
            lidar_subscribe_=this->create_subscription<std_msgs::msg::Float32MultiArray>(
                "lidar_dis_angle", 10, 
                std::bind(&ControlPlaneV2::lidar_callback, this, std::placeholders::_1));
            // 订阅imu主题
            imu_sub_=this->create_subscription<sensor_msgs::msg::Imu>(
                "imu/data_raw",10,std::bind(&ControlPlaneV2::imu_callback,this,std::placeholders::_1));
            // 创建一个订阅者L订阅话题
            command_subscribe_L = this->create_subscription<std_msgs::msg::Float32>(
                "esp_pro_l", 10, std::bind(&ControlPlaneV2::vL_callback, this, std::placeholders::_1));
            // 创建一个订阅者R订阅话题
            command_subscribe_R = this->create_subscription<std_msgs::msg::Float32>(
                "esp_pro_r", 10, std::bind(&ControlPlaneV2::vR_callback, this, std::placeholders::_1));
            // 计算的vL订阅话题
            command_vL = this->create_subscription<std_msgs::msg::Float32>(
                "cal_vel_l", 10, std::bind(&ControlPlaneV2::cal_vL_callback, this, std::placeholders::_1));
            // 计算的vR订阅话题
            command_vR = this->create_subscription<std_msgs::msg::Float32>(
                "cal_vel_r", 10, std::bind(&ControlPlaneV2::cal_vR_callback, this, std::placeholders::_1));
            //
            QObject::connect(ui_.RFNN_start, &QPushButton::clicked, [this](){ this->onRFNNStart(); });
            QObject::connect(ui_.ESP32_start, &QPushButton::clicked, [this](){ this->onESP32Start(); });
            QObject::connect(ui_.switch_off, &QPushButton::clicked, [this](){ this->onSwitchOff(); });
            QObject::connect(ui_.power_off, &QPushButton::clicked, [this](){ this->onPowerOff(); });
            window_.show();
        }
};

// void Ui::control_plane::updateLabels(QString dg_val, QString sig_val, QString do_val, QString sio_val)
// {
//     dg_val->setText(dg_val);
//     sig_val->setText(sig_val);
//     do_val->setText(do_val);
//     sio_val->setText(sio_val);
// }

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    QApplication app(argc,argv);
    auto node = std::make_shared<ControlPlaneV2>("control_plane_v2");
    // 创建一个定时器来处理Qt事件
    QTimer::singleShot(0,[node,&app]() {
        while (rclcpp::ok())
        {
            app.processEvents();
            rclcpp::spin_some(node);
            QThread::msleep(10);
        }
    });
    int result = app.exec();
    // rclcpp::spin(node);  // 保持节点运行
    rclcpp::shutdown();
    return result;
}